
#include <iostream>
#include <unistd.h>
#include <ros/ros.h>

#include "com_ctrl.h"
#include "controlcan.h"
#include "tail_motor_data.h"
#include "motor_ctrl.h"
#include "tail_control.h"

using namespace std;

USBCAN_Ctrl::USBCAN_Ctrl(void *param, uint8_t len):Com_Ctrl(COM_USBCAN, param, len)
{
    cout<<"USBCAN_Ctrl"<<endl;
}

USBCAN_Ctrl::~USBCAN_Ctrl()
{
    cout<<"~USBCAN_Ctrl"<<endl;
}

int32_t USBCAN_Ctrl::send_func(void *pf, uint8_t num)
{
    CAN_Frame_Desc *pframe = (CAN_Frame_Desc *)pf;
    uint8_t group = 0;
    uint8_t chn = 0;
    uint8_t cnt[USB_TOOL_MAX]= {0};
    for(uint8_t index=0; (index<num) && (index<USBCAN_SENDFRAME_MAX); index++) {
        group = pframe[index].Group;
        chn = pframe[index].Chn;
        if((pframe[index].DLC == 0) || (group != 0) || (chn != 0)) {    //暂时只支持一个USBCAN工具
            continue;
        }
        if(pframe[index].IDE == CAN_ID_STD) {
            send[index].ExternFlag = 0;
            send[index].ID = pframe[index].StdId; 
        } else {
            send[index].ExternFlag = 1; 
            send[index].ID = pframe[index].ExtId; 
        }  
        if(pframe[index].RTR == CAN_RTR_DATA) {
            send[index].RemoteFlag = 0;  
        } else {
            send[index].RemoteFlag = 1; 
        }
        send[index].SendType = 0;
        send[index].DataLen = pframe[index].DLC;
        for(uint8_t i = 0; i < pframe[index].DLC; i++) {
            send[index].Data[i] = pframe[index].Data[i];
        }
        cnt[group] += 1;
#if 0
        if(VCI_Transmit(VCI_USBCAN2, 0, 0, &send[index], 1) == 1) {
            ;
        } else {
            cout << "can device id may not exist, or usb_can is offline!" << endl;
            return -1;            
        }
    }
#else
    }
    for(uint8_t i=0; i<USB_TOOL_MAX; i++) {
        if(cnt[i] == 0) {
            continue;
        }
        if(VCI_Transmit(VCI_USBCAN2, 0, 0, &send[0], num) == num) {
            return 0;
        } else {
            cout << "can device id may not exist, or usb_can is offline!" << endl;
            return -1;
        }
    }
#endif    
    return 0;
}

extern std::shared_ptr<TailControl> control;
// extern TailMotorData motor_data_ori[TAIL_MOTOR_NUM];
// extern uint8_t vesc_round;
void *USBCAN_Ctrl::com_receive_loop()
{   
    uint32_t reclen = 0;
    uint8_t motor_id;
    uint8_t index;
    uint8_t num = 0;
    while(1 != this->com_thread_getflag()) {
        sleep(1);
    }
    while(1 == this->com_thread_getflag()) {
        usleep(10);
        if((reclen = VCI_Receive(VCI_USBCAN2, 0, 0, recv, USBCAN_RECVFRAME_MAX, 100)) > 0) {
        #if 0
            for(uint32_t i = 0; i < reclen; i++) {
                cout << "CAN1";
                printf(" RX_ID:0x%x",recv[i].ID);
                if(recv[i].ExternFlag==0)  { // 帧格式：标准帧 or 扩展帧
                    cout <<" Standard ";
                } else {
                    cout <<" Extend ";
                }

                if(recv[i].RemoteFlag==0) {  // 帧类型：数据帧 or 远程帧
                    cout <<" Data ";   
                } else  {
                    cout << " Remote ";  
                }

                cout << " DLC:" << recv[i].DataLen;   //帧长度
                printf(" data:0x");	                            //数据
                for(uint8_t j = 0; j < recv[i].DataLen; j++) {
                    printf(" %02X", recv[i].Data[j]);
                }
                printf(" TimeStamp:0x%08X\n",recv[i].TimeStamp);  //时间标识
            }
        #else
            for(uint32_t i = 0; i < reclen; i++) {
                if(recv[i].RemoteFlag == 0) { // 帧类型：数据帧
                    if(recv[i].ExternFlag == 1) {   //标准帧为YBT, 扩展帧为AK或VESC
                        motor_id = recv[i].ID & 0xff;

                        for(num = 0; num < control->p_motor_ctrl->get_motor_num(); num++) {
                            if(control->p_motor_ctrl->get_motor_item(num)->get_motor_id() == motor_id) {
                                break;    
                            }
                        }
                        if(num == control->p_motor_ctrl->get_motor_num()) {
                            continue;
                        }
                        if((motor_id>0) && (motor_id<4)) {
                            if((((recv[i].ID>>8) & (0xFF)) == VESC_STATUS_16)) {
                                // int16_t pos_int = ((recv[i].Data[6] << 8) | (recv[i].Data[7]));
                                // motor_data_ori[motor_id-1].pos = ((pos_int/50 + vesc_round*360)/180.0f*3.14);
                                // printf("VESC:%d, [pos:%f]\n", motor_id, pos_int/50 + vesc_round*360);
                                pthread_mutex_lock(&control->mutex_lock);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_pos(&recv[i].Data[6]);
                                pthread_mutex_unlock(&control->mutex_lock);
                                printf("VESC:%d, cur_pos:%f\n", motor_id, control->p_motor_ctrl->get_motor_item(num)->get_cur_pos());

                            } else if((((recv[i].ID>>8) & (0xFF)) == VESC_STATUS_9)) {
                                // int32_t spd_int = (recv[i].Data[0]<<24)|(recv[i].Data[1]<<16)|(recv[i].Data[2]<<8)|(recv[i].Data[3]);
                                // int16_t cur_int = ((recv[i].Data[4] <<8)| recv[i].Data[5]);
                                // motor_data_ori[motor_id-1].vel = spd_int/VESC_RATIO/VESC_POLES/60*6.28f;
                                // motor_data_ori[motor_id-1].tau = abs(cur_int)/10.0f*VESC_T_CONST*VESC_RATIO;
                                //  printf("VESC:%d, [vel:%f tau:%f]\n", motor_id, motor_data_ori[motor_id-1].vel/6.28f*360.0f, motor_data_ori[motor_id-1].tau);
                                pthread_mutex_lock(&control->mutex_lock);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_w(&recv[i].Data[0]);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_tau(&recv[i].Data[4]);
                                pthread_mutex_unlock(&control->mutex_lock);
                                //  printf("VESC:%d, cur_w:%f cur_tau:%f\n", motor_id, 
                                //         control->p_motor_ctrl->get_motor_item(num)->get_cur_w(),
                                //         control->p_motor_ctrl->get_motor_item(num)->get_cur_tau());
                            } else if((((recv[i].ID>>8) & (0xFF)) == AK_STATUS)) {
                                // int16_t pos_int = ((recv[i].Data[0] << 8) | (recv[i].Data[1]));   //电机顺时针为位置增大方向
                                // int16_t spd_int = ((recv[i].Data[2] << 8) | (recv[i].Data[3]));   //电机逆时针为速度增大方向  
                                // int16_t cur_int = ((recv[i].Data[4] << 8) | (recv[i].Data[5]));   //电机逆时针为扭矩增大方向(电机反馈的扭矩为绝对值)
                                // motor_data_ori[motor_id-1].pos =  (pos_int/10.0f)*3.14f/180;
                                // motor_data_ori[motor_id-1].vel =  -(spd_int*10.0f)/AK_60_6_V_RATIO/AK_60_6_POLES/60*6.28f;
                                // motor_data_ori[motor_id-1].tau =  (cur_int/100.0f)*AK_60_6_T_CONST*AK_60_6_V_RATIO; 
                                // printf("AK MOTOR%d [pos:%f, vel:%f, tau:%f]\n", motor_id, (pos_int/10.0f), (motor_data_ori[motor_id-1].vel/6.28f*360.0f), motor_data_ori[motor_id-1].tau);                     
                                pthread_mutex_lock(&control->mutex_lock);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_pos(recv[i].Data);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_w(&recv[i].Data[2]);
                                control->p_motor_ctrl->get_motor_item(num)->set_cur_tau(&recv[i].Data[4]);
                                pthread_mutex_unlock(&control->mutex_lock);
                                // printf("AK:%d, cur_pos:%f cur_w:%f cur_tau:%f\n", motor_id, 
                                //         control->p_motor_ctrl->get_motor_item(num)->get_cur_pos(),
                                //         control->p_motor_ctrl->get_motor_item(num)->get_cur_w(),
                                //         control->p_motor_ctrl->get_motor_item(num)->get_cur_tau());
                            
                            } else {
                                ;
                            }
                        }
                    } else {
                        ;
                    }
                }

            }

        #endif
        }
        
    }
    printf("usbcan thread exit\n");//退出接收线程
    return NULL; 
}


int32_t USBCAN_Ctrl::com_ctrl_init()
{
    this->baudrate = ((uint32_t *)get_com_param())[0];
    if(this->baudrate != USBCAN_BAUD_1M) {
        cout << ">>USBCAN baudrate error!" << endl;
        return -1;    
    }

    int num = VCI_FindUsbDevice2(pInfo);
    cout << ">>USBCAN DEVICE NUM:" << num << " PCS" << endl;

    for(int i = 0; i < num; i++){
        cout << "Device:" << i << endl;
        cout << ">>Get VCI_ReadBoardInfo success!" << endl;   
        cout << ">>Serial_Num:" << pInfo[i].str_Serial_Num << endl;
        cout << ">>hw_Type:" << pInfo[i].str_hw_Type << endl;
        cout << ">>Firmware Version:V";
        printf("%x.%x%x\n", (pInfo[i].fw_Version&0xF00)>>8, (pInfo[i].fw_Version&0xF0)>>4, pInfo[i].fw_Version&0x0F);
    }

    cout << ">>" << endl;
    cout << ">>" << endl;
    cout << ">>" << endl;

    if(VCI_OpenDevice(VCI_USBCAN2, 0, 0) == 1) {   //打开通道0
        cout << ">>open deivce success!" << endl;
    } else {
        cout << ">>open deivce error!" << endl;
        return -1;
    }

    //初始化参数，严格参数二次开发函数库说明书。
    VCI_INIT_CONFIG config;
    config.AccCode  = 0;
    config.AccMask  = 0xFFFFFFFF;
    config.Filter   = 1;        //接收所有帧
    config.Timing0  = 0x00;    /*1M bps  0x00  0x14*/
    config.Timing1  = 0x14;
    config.Mode     = 0;          //正常模式		
    
    if(VCI_InitCAN(VCI_USBCAN2, 0, 0, &config) != 1){
        cout << ">>Init CAN1 error!" << endl;
        VCI_CloseDevice(VCI_USBCAN2,0);
        return -1;
    }

    if(VCI_StartCAN(VCI_USBCAN2, 0, 0) != 1)
    {
        cout << ">>Start CAN1 error!" << endl;
        VCI_CloseDevice(VCI_USBCAN2,0);
        return -1;

    }

    this->Com_Ctrl::com_ctrl_init();  
    return 0; 
}

int32_t USBCAN_Ctrl::com_ctrl_deinit()
{
    this->Com_Ctrl::com_ctrl_deinit();

    if(VCI_ResetCAN(VCI_USBCAN2, 0, 0) == 1) {       //复位CAN0通道
        cout << ">>reset deivce success!" << endl;
    } else {
        cout << ">>reset deivce error!" << endl;
        return -1;
    }

    if(VCI_CloseDevice(VCI_USBCAN2,0) == 1) {       //关闭设备
        cout << ">>close deivce success!" << endl;
    } else {
        cout << ">>close deivce error!" << endl;
        return -1;
    }

    return 0;
}